package behavior;

import java.util.ArrayList;
import java.util.Hashtable;

import Interface.BluetoothNXT;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.robotics.navigation.TachoPilot;

public class BarcodeAnalyser extends Analyser {

	private LightSensor sensor;
	private TachoPilot pilot;
	
	private Hashtable barcodeMap;
	private ArrayList<String> barcodes = new ArrayList<String>();
	private ArrayList<Integer> readings = new ArrayList<Integer>();
	private ArrayList<Integer> colors = new ArrayList<Integer>();
	
	private int brownValue;
	private int whiteMargin=20;
	private int blackMargin=25;
	private boolean frontPressed=false;
	private boolean backReleased = false;
	
	private TouchSensor frontTouch;
	private TouchSensor backTouch;
	private BluetoothNXT bt;
	private boolean bluetooth = false;
	private float distance;
	private int speed = 350;

	public BarcodeAnalyser(TachoPilot pilot) {
		this.pilot=pilot;
		sensor = new LightSensor(SensorPort.S1);
		frontTouch= new TouchSensor(SensorPort.S3);
		backTouch = new TouchSensor(SensorPort.S4);
		LCD.clear();
		barcodeMap = new Hashtable();
		setupBarcodes();

	}
	
	public BarcodeAnalyser(TachoPilot pilot,BluetoothNXT bt) {
		this.pilot=pilot;
		sensor = new LightSensor(SensorPort.S1);
		frontTouch= new TouchSensor(SensorPort.S3);
		backTouch = new TouchSensor(SensorPort.S4);
		LCD.clear();
		barcodeMap = new Hashtable();
		setupBarcodes();
		bluetooth = true;
		this.bt = bt;
	}

	public int analyse() {
		pilot.reset();
		Motor.B.setSpeed(speed);
		Motor.C.setSpeed(speed);
		frontPressed=false;
		backReleased=false;
		brownValue = sensor.getNormalizedLightValue();
		distance = 0;
		pilot.forward();
		int counter = 0;
		Integer barcodeNb = 0;

		while (counter < 3) {
			distance=pilot.getTravelDistance();
			if(Math.abs(distance)
					> 80)
			{
				pilot.stop(); //No bc detected
				return 0;
			}
			
			if (getColorFromValue(sensor.getNormalizedLightValue()) != 2)
				counter++;
			else
				counter = 0;
		}

		ArrayList<Integer> readings = getBarcodeReading();

		if (readings != null) {
			String detectedBarcode = calculateBarcode(readings);
			barcodeNb = (Integer) barcodeMap.get(detectedBarcode);
			if (barcodeNb != null) {
				LCD.drawString("" + barcodeNb, 0, 3);

				// System.out.println(barcodeNb);
			} else {
				barcodeNb = (Integer) barcodeMap
						.get(invertBarcode(detectedBarcode));
				if (barcodeNb == null) {
					System.out.println("null");
				} else
					pilot.rotate(180);
			}

			
			if(barcodeNb == null)
				barcodeNb = recover2(detectedBarcode, barcodes);
						
		}
		
		pilot.stop();
		distance = pilot.getTravelDistance();
		pilot.travel((80 - distance));
		
		if(barcodeNb == null)
			return 0;
		else  {
			
			if (bluetooth)
			{
				bt.setBarcode(barcodeNb);
			}
			
			return barcodeNb;
		}
			

	}

	/**
	 * Inverts the given string and returns the inversion
	 */
	private Object invertBarcode(String detectedBarcode) {
		String ret = "";
		for (int i = detectedBarcode.length() - 1; i >= 0; i--) {
			ret += detectedBarcode.charAt(i);
		}
		return ret;
	}

	/**
	 * Returns whether the detected lightvalue is black (0), white(1) or
	 * neutral(2)
	 */
	private int getColorFromValue(int color){
		if (color > brownValue+whiteMargin){
			return 1;
		}
		if(color < brownValue-blackMargin){
			return 0;
		}
		else{
			return 2;
		}
	}

	/**
	 * Returns whether the detected lightvalue is black (0), white(1)
	 */
	private int getColorFromValueBin(int value) {
		// Detected white
		if (value > brownValue)
			return 1;
		// Detected black
		else
			return 0;
	}

	/**
	 * Transforms the data from the lightsensor into strings representing a
	 * barcode. Returns null if the crossed underground wasn't a barcode (ie it
	 * could be a sideline).
	 * 
	 */
	private ArrayList<Integer> getBarcodeReading() {
		readings.clear();

		int currentReading = 0;
		int neutralValueCount = 0;
		int backwardTouchCount=0;

		while (neutralValueCount < 250) {
			currentReading = sensor.getNormalizedLightValue();
			if(frontTouch.isPressed())
				frontPressed=true;
			if(!backTouch.isPressed())
				backwardTouchCount++;
			else 
				backwardTouchCount=0;
			if(backwardTouchCount>=5)
				backReleased=true;
			System.out.println(getColorFromValue(currentReading));
			if (getColorFromValue(currentReading) == 2)
				neutralValueCount++;
			else
				neutralValueCount = 0;

			readings.add(currentReading);

			if (readings.size() > 800) {
				System.out.println("Too long");
				return null;
			}

		}

		pilot.stop();

		// remove the last 250 (neutral) values from the readings
		for (int i = 0; i < 250; i++) {
			readings.remove(readings.size() - 1);
		}
		
		// System.out.println("readings.size= " + readings.size());

		if (readings.size() < 150) {
			System.out.println("Too short");
			return null;
		}

		return readings;
	}

	/**
	 * Returns a string which contains a barcode derived from given readings. It
	 * divides the readings in 7 equal blocks and calculates the mean of each
	 * block.
	 */
	private String calculateBarcode(ArrayList<Integer> readings) {

		int barlength = Math.round(readings.size() / 7);
		colors.clear();

		for (int i = 0; i < 7; i++) {

			double tempsum = 0;
			for (int j = 0; j < barlength; j++) {
				tempsum += readings.get(i * barlength + j);
			}
			double meanValue = tempsum / barlength;

			colors.add(getColorFromValueBin((int) meanValue));
		}

		// prepare string to return
		String ret = "";
		for (int i = 0; i < colors.size(); i++) {
			int color = colors.get(i);
			ret = ret + color;
		}
		System.out.println("gelezen string: " + ret);

		return ret;
	}


	/**
	 * Adds barcode-data to the barcodeMap See the online pdf for barcode
	 * numbers
	 */
	private void setupBarcodes() {
		barcodeMap.put("0001111", 1); // straight
		barcodeMap.put("0110011", 6); // right turn
		barcodeMap.put("0011001", 3); // left turn
		barcodeMap.put("0010110", 2); // up
		barcodeMap.put("0100101", 4); // down
		barcodeMap.put("0111100", 7); // wip
		barcodeMap.put("1000011", 8); //versmalling
		barcodes.add("0001111"); // straight
		barcodes.add("0110011"); // right
		barcodes.add("0011001"); // left
		barcodes.add("0010110"); // up
		barcodes.add("0100101"); // down
		barcodes.add("0111100"); // wip
		barcodes.add("1000011"); // versmalling

	}

	public void testReadings(){

		System.out.println("executing testreadings");
		while (!Button.ENTER.isPressed()) {

			int reading = sensor.getNormalizedLightValue();
			System.out.println("real: " + reading);
			System.out.println("fake: " + getColorFromValue(reading));
			sleep(300);

		}
		LCD.clear();
	}

//	private void sleep(int millisec) {
//		try {
//			Thread.sleep(millisec);
//		} catch (InterruptedException e) {
//			System.out.println("Sleep exception");
//		}
//	}

	private int recover2(String bc, ArrayList<String> rec) {
		System.out.println("Recovery 2");

		String recoveredBc = "";
		int minDist = 100;
		for (int i = 0; i < rec.size(); i++) {
			int hammingDist = getHammingDist(bc, rec.get(i));

			if (hammingDist < minDist) {
				minDist = hammingDist;
				recoveredBc = rec.get(i);
			}
		}
		
		if(minDist > 1)
			return 0;
		
		Integer ret = (Integer) barcodeMap.get(recoveredBc);		
		
		return ret;
	}

	private int getHammingDist(String s1, String s2) {
		int ret = 0;

		for (int i = 0; i < s1.length(); i++) {
			if (s1.charAt(i) != s2.charAt(i))
				ret++;
		}
		return ret;
	}
	
	public boolean frontIsPressed(){
		return frontPressed;
	}
	public boolean backReleased(){
		return backReleased;
	}

	

	

}
